package rawengine.physics;

import org.lwjgl.util.vector.*;

public class SimpleProjectile3D extends ODE 
{
  //  gravitational acceleration.
  public final static float G = -9.81f;
  
  protected Vector3f position = new Vector3f();	
  protected Vector3f velocity = new Vector3f();
  
  
  public SimpleProjectile3D(float x0, float y0, float z0, 
                          float vx0, float vy0, float vz0,
                          float time) {
    //  Call the ODE class constructor
    super(6);

    //  Load the initial position, velocity, and time 
    //  values into the s field and q array from the
    //  ODE class.
    setS(time);
    setQ(vx0,0);
    setQ(x0, 1);
    setQ(vy0,2);
    setQ(y0, 3);
    setQ(vz0,4);
    setQ(z0, 5);
  }

  public SimpleProjectile3D(Vector3f position, Vector3f velocity) {
    //  Call the ODE class constructor
    super(6);

    //  Load the initial position, velocity, and time 
    //  values into the s field and q array from the
    //  ODE class.
    //setS(time);
    this.position = position;
    this.velocity = velocity;
    
    setQ(velocity.x, 0);
    setQ(position.x, 1);
    setQ(velocity.y, 2);
    setQ(position.y, 3);
    setQ(velocity.z, 4);
    setQ(position.z, 5);
  }	

  //  These methods return the location, velocity, 
  //  and time values
  public float getVx() {
    return getQ(0);
  }

  public float getVy() {
    return getQ(2);
  }

  public float getVz() {
    return getQ(4);
  }

  public float getX() {
    return getQ(1);
  }

  public float getY() {
    return getQ(3);
  }

  public float getZ() {
    return getQ(5);
  }

  public float getTime() {
    return getS();
  }

  //  This method updates the velocity and position
  //  of the projectile according to the gravity-only model.
  public void update(float dt) {
    //  Get current location, velocity, and time values
    //  From the values stored in the ODE class.

   /*
    float time = getS();
    float vx0 = getQ(0);
    float x0 = getQ(1);
    float vy0 = getQ(2);
    float y0 = getQ(3);
    float vz0 = getQ(4);
    float z0 = getQ(5);
   */
    position.x = getQ(1);	
    position.y = getQ(3);
    position.z = getQ(5);

    velocity.x = getQ(0);
    velocity.y = getQ(2);
    velocity.z = getQ(4);
	
    //  Update the xyz locations and the z-component
    //  of velocity. The x- and y-velocities don't change.
    /*
    float x = x0 + vx0*dt;
    float y = y0 + vy0*dt;
    float vz = vz0 + G*dt;
    float z = z0 + vz0*dt + 0.5f*G*dt*dt;
    */
    
    position.x = position.x + velocity.x*dt;
    position.z = position.z + velocity.z*dt;  
    velocity.y = velocity.y + G*dt;	
    position.y = position.y + velocity.y*dt + 0.5f*G*dt*dt;	 

    //  Update time;
    //time = time + dt;

    //  Load new values into ODE arrays and fields.
    /*
    setS(time);
    setQ(x, 1);
    setQ(y, 3);
    setQ(vz,4);
    setQ(z, 5);
    */
    setS(dt);	
    setQ(position.x, 1);
    setQ(position.y, 3);
    setQ(velocity.y, 2);
    setQ(position.z, 5); 	

  }

  //  Because SimpleProjectile extends the ODE class,
  //  it must implement the getRightHandSide method.
  //  In this case, the method returns a dummy array.
  public float[] getRightHandSide(float s, float Q[], 
               float deltaQ[], float ds, float qScale) {
    return new float[1];
  }
}
